The present invention relates to a robot control apparatus, and more particularly to a robot control apparatus in which the actual spatial position of an operating member is made to be completely coincident with the stored data representing the position.
FIGS. 1A and 1B are schematic diagrams showing a robot of the two-arm horizontal articulation type, viewed from above, as an example to which the present invention is applied. In FIGS. 1A and 1B, reference numeral 1 designates a first arm which rotates about a first shaft (or pin) 3 provided at one end of the first arm 1, and 2 designates a second arm which rotates about a second shaft 4 rotatably provided at the other end of the first arm 1. By rotating the first and second arms 1 and 2 through predetermined angles, the other end of the second arm 2 can be brought into a predetermined desired operating position. Further, reference numeral 5 designates a position confirmation switch mechanically fixed to the first shaft 3 and used for establishing a reference point of the first arm 1, 6 designates a dog attached to the first arm 1 on the side of the first shaft 3 and on the center line of the first arm 1 corresponding to the position confirmation switch 5, 7 designates a position confirmation switch mechanically fixed to the first arm 1 at its end on the side of the second arm 2 for establishing a reference point of the second arm 2, and 8 designates a dog attached to the second arm 2 on the side of the first arm 1 and on the center line of the second arm 2 corresponding to the position confirmation switch 7. Thus, the pair of the position confirmation switch 5 and the dog 6 are disposed opposite one another on the center line of the first arm 1, and the pair of the position confirmation switch 7 and the dog 8 are disposed opposite one another on the center line of the first arm 2. FIG. 1B shows the state of the robot in which the respective arms 1 and 2 are returned to their reference points.
The operation of a robot 9 having such an arrangement as described above is controlled by a control device 10 as shown in FIG. 2. In FIG. 2, reference numeral 11 designates a main storage section for storing the data of work points, the work procedure, etc., of the robot, 13 designates a reference point storage section for storing the positional data of the respective arms 1 and 2 when they are returned to their reference points, 14 is a central processing unit (CPU) for effecting various control routines such as positional control for the robot 9 and control of external apparatuses, and 15 designates an instruction output section for providing an output to a servomotor of the robot 9. On the basis of the work point data and the work procedure stored in the main storage section 11, the present position data stored in the present point storage section 12, and the reference position data stored in the reference point storage section 13, the CPU 14 computes and sends instructions to the robot 9 through the instruction output section 15 so as to cause the robot 9 to perform predetermined desired movements. The positional data stored in the present point storage section 12 and the reference point storage section 13 includes, for example, as shown in FIG. 3, data representing the angle .theta..sub.1 between the X axis of a reference orthogonal coordinate system and the first arm 1 (more specifically, the center line of the first arm 1) and the angle .theta..sub.2 between an extension line (broken line) of the first arm 1 and the second arm 2.
In the thus arranged conventional robot control device, when the operational control is initiated (power-on time), it is necessary to confirm the positions of the arms 1 and 2 and store them in the present point storage section 12 because the content of the present point storage section 12 is then in a blank state. For this, in starting, reference point return is effected whereby the actual spatial positions of the respective arms 1 and 2 are caused to agree with the stored data of the respective arms 1 and 2. For example, if the arms 1 and 2 are in the position shown in FIG. 1A, the arm 1 is rotated about the shaft 3 until the position confirmation switch 5 aligns with the dog 6, and at the same time the arm 2 is rotated about the shaft 4 until the position confirmation switch 7 aligns with the dog 8, thereby bringing the arms 1 and 2 to their reference points as shown in FIG. 1B.
Upon the confirmation of alignment between the pair of the position confirmation switch 5 and the dog 6 and between the pair of the position confirmation switch 7 and the dog 8, the respective arms 1 and 2 are in their reference point returned states as shown in FIG. 1B. Upon the detection of alignment between the pair of the position confirmation switch 5 and the dog 6 and between the pair of the position confirmation switch 7 and the dog 8, the CPU 14 transfers the positional data .theta..sub.10 and .theta..sub.20 stored in the reference point storage section 13 to the present point storage section 12, and the actual spatial positions of the respective arms 1 and 2 are then made coincident with the stored positional data. Since the position confirmation switches 5 and 7 and the dogs 6 and 8 are mechanically fixed, the positions of the respective arms 1 and 2 do not vary when they are returned to their reference points. Thereafter, positional control of the robot is performed on the basis of the positional data stored in the present point storage section 12 and in accordance with the work procedure stored in the main storage section 11. That is, when the tip (free) end of the composite robot arm is to be moved to a predetermined work point according to the procedure stored in the main storage section 11, the amount of movement of each of the arms 1 and 2 is computed by the CPU 14 such that the present position data, which is stored in the present point storage section 12 and which is successively updated in accordance with the movement, agrees with the positional data of the predetermined work point.
For positional control of robot to be performed in the manner as described above, initial agreement between the actual spatial positions of the reference points of the arms 1 and 2 and the stored positional data of the reference points is a prerequisite. It is difficult, however, to attach the position confirmation switches and the dogs at the positions of their reference points with a high accuracy, even if the positions of their reference points are accurately determined in the design stage, because of errors in assembly and/or manufacturing tolerances. In the case where disagreement between such positions exists, that is when the actual spatial positions .theta..sub.11 and .theta..sub.21 (FIG. 4, solid line) do not agree with the positional data .theta..sub.12 and .theta..sub.22 (FIG. 4, broken line) stored in the reference point storage section 13, in order to move the tip end of the robot in the positive direction of the Y axis and parallel to the Y axis, the CPU 14 carries out its computations on the basis of the stored date .theta..sub.12 and .theta..sub.22, thereby causing the instruction output section 15 to produce an instruction which causes the tip end of the robot arm to pass through the linear path between the points a and b as shown by the double-dot chain line in FIG. 4. Practically, however, there is a problem in that the tip end of the robot arm passes along a locus from the point A to the point B as shown by single-dot chain line in FIG. 4 because the actual operational start point A is at the position determined by the data values .theta..sub.11 and .theta..sub.21, and thus it is impossible to carry out the desired work accurately.
The reason why the locus becomes a curve will now be described. When a certain point P (X,Y) in a plane is instructed as a position of the tip end of the robot arm as shown in FIG. 5, there exists the following relations among the values X, Y, .theta..sub.1 and .theta..sub.2 : ##EQU1## The values .theta..sub.1 and .theta..sub.2 can be obtained from the equation (1) as follows: ##EQU2## Although there are two solutions for the value of .theta..sub.2, that is, one for both the right-and left-hand systems, the following description relates only to the right-hand system (+.theta..sub.2). The case of the left-hand system is similar to the right-hand one.
Assume now that the value of point (X.sub.0,Y.sub.0) is instructed, and the angles of the respective articulations are expressed from equations (2) and (3) as follows: ##EQU3## Similarly to this, the angles of the respective articulations when a point (X.sub.0 +.DELTA.X, Y.sub.0 +.DELTA.Y), shifted by (.DELTA.X, .DELTA.Y) from the point (X.sub.0,Y.sub.0), is instructed are obtained as follows: ##EQU4## Similarly, when a point (X.sub.0 +2.multidot..DELTA.X,Y.sub.0 +2.multidot..DELTA.Y) is instructed, ##EQU5## The tip end of the robot arm will pass through these three points if the robot is an ideal one. Practically, however, as explained above, a real robot will have some deviation between the actual position and the stored point. This deviation is a difference between the position of the reference point of each articulation and the value of the coordinates of the reference point of the robot arm expressed by .DELTA..theta..sub.1, and .DELTA..theta..sub.2. Accordingly, for desired values (positions) (.theta..sub.10, .theta..sub.2.sbsb.0), (.theta..sub.1.sbsb.1, .theta..sub.2.sbsb.1) and (.theta..sub.2.sbsb.1, .theta..sub.2.sbsb.2), obtained as results of computing, the actual position is found from equation (1) to be: ##EQU6## If .DELTA..theta..sub.1 =0 and .DELTA..theta..sub.2 =0, the robot is ideal and the robot arm will pass through the three points (X.sub.0,Y.sub.0), (X.sub.0 +.DELTA.X, Y.sub.0 +.DELTA.Y) and (X.sub.0 +2.DELTA.X,Y.sub.0 +2.DELTA.Y). However, when .DELTA..theta..sub.1 .noteq.0 and .DELTA..theta..sub.2 .noteq.0, components due to the values .DELTA..theta..sub.1 and .DELTA..theta..sub.2 are added, and therefore the three points are shifted from the desired locus. Thus, the locus becomes the curve AB. In order to eliminate this error, it has been the practice to make the data points coincident by actually measuring the distance of actual movement of the robot by using an expensive jig, for example, such as a digitizer, for focusing agreement between the spatial position of the reference point and the stored positional data of the reference point.